import numpy as np
import cv2

####-----用于计算两点之间的距离函数，进行两点之间插补计算
def DistanceCalculation(Points):
    coornates = []
    for i in range(Points.shape[0] - 1):
        point1 = Points[i]
        point2 = Points[i + 1]
        Distance = sum(abs(point2 - point1))
        for i in range(int(Distance)):
            x = point2[0] - point1[0]
            y = point2[1] - point1[1]
            z = point2[2] - point1[2]
            posture = ([(point1[0] + i * x / Distance), (point1[1] + i * y / Distance), (point1[2] + i * z / Distance),
                        179,-1,-65]) #绝对坐标位姿，x,y,z,r,p,y
            coornates.append(posture)
    return coornates
"""
   机器人反解代码
   输出六个关节以弧度为单位的角度
"""
def URIkine(DHtable, coodirate):
    Fjiao = np.empty((0,6))
    angle = np.empty((0,6))
    error = 0
    for i in range(coodirate.shape[0]+1):  # 循环求各个插补点的反解
        Pose = coodirate[i, :]
        # 使用z-y-x欧拉角
        # 获取基座标系到6坐标系的欧拉角,并将其转化为弧度制
        Z6radian = Pose[5] * np.pi / 180
        Y6radian = Pose[4] * np.pi / 180
        X6radian = Pose[3] * np.pi / 180
        # 求解出目标坐标系和基坐标系之间的位姿变换矩阵
        # Rzyx(zdegree,ydegree,xdegree)=Rz(zdegree)*Ry(ydegree)*Rx(xdegree)
        T = np.array([[np.cos(Z6radian) * np.cos(Y6radian),
                       np.cos(Z6radian) * np.sin(Y6radian) * np.sin(X6radian) - np.sin(Z6radian) * np.cos(X6radian),
                       np.cos(Z6radian) * np.sin(Y6radian) * np.cos(X6radian) + np.sin(Z6radian) * np.sin(X6radian),
                       Pose[0]],
                      [np.sin(Z6radian) * np.cos(Y6radian),
                       np.sin(Z6radian) * np.sin(Y6radian) * np.sin(X6radian) + np.cos(Z6radian) * np.cos(X6radian),
                       np.sin(Z6radian) * np.sin(Y6radian) * np.cos(X6radian) - np.cos(Z6radian) * np.sin(X6radian),
                       Pose[1]],
                      [-np.sin(Y6radian), np.cos(Y6radian) * np.sin(X6radian), np.cos(Y6radian) * np.cos(X6radian),
                       Pose[2]],
                      [0, 0, 0, 1]])
        # 反解出基坐标系到六坐标系的变换矩阵，先解出六坐标系到目标坐标系的变换矩阵
        d1 = DHtable[0, 1]  # - 100
        a2 = DHtable[1, 2]
        a3 = DHtable[2, 2]
        d4 = DHtable[3, 1]
        d5 = DHtable[4, 1]
        # %%%%%%
        theta7 = DHtable[5, 0]
        a6 = DHtable[5, 2]
        alpha6 = DHtable[5, 3]
        d7 = DHtable[5, 1]
        T67 = np.array([[np.cos(theta7), -np.sin(theta7), 0, a6],
                        [np.sin(theta7) * np.cos(alpha6), np.cos(theta7) * np.cos(alpha6), -np.sin(alpha6),
                         -np.sin(alpha6) * d7],
                        [np.sin(theta7) * np.sin(alpha6), np.cos(theta7) * np.sin(alpha6), np.cos(alpha6),
                         np.cos(alpha6) * d7],
                        [0, 0, 0, 1]])
        T06 = np.dot(T, np.linalg.inv(T67))  # 矩阵乘法

        # 将T06各元素提取出来赋值
        nx = T06[0, 0]
        ny = T06[1, 0]
        nz = T06[2, 0]
        ox = T06[0, 1]
        oy = T06[1, 1]
        oz = T06[2, 1]
        ax = T06[0, 2]
        ay = T06[1, 2]
        az = T06[2, 2]
        px = T06[0, 3]
        py = T06[1, 3]
        pz = T06[2, 3]

        # 初始化itheta矩阵
        itheta = np.empty(shape=(8, 6))
        # 定义变量k,其代表逆解的行名称
        k = -1

        # 求解theta1
        ForJudgment = px ** 2 + py ** 2 - d4 ** 2

        if ForJudgment < -1e-6:
            error = 999  # 超出工作空间，无法求解
            itheta = ''
        else:
            # 求解theta1
            if -1e-6 <= ForJudgment < 0:
                ForJudgment = 0
            theta1_1 = np.arctan2(py, px) - np.arctan2(-d4, np.sqrt(ForJudgment))
            theta1_2 = np.arctan2(py, px) - np.arctan2(-d4, -np.sqrt(ForJudgment))

            # 求解theta5
            S5_1 = np.sqrt((-np.sin(theta1_1) * nx + np.cos(theta1_1) * ny) ** 2 +
                           (-np.sin(theta1_1) * ox + np.cos(theta1_1) * oy) ** 2)
            theta5_1 = np.arctan2(S5_1, np.sin(theta1_1) * ax - np.cos(theta1_1) * ay)
            S5_2 = -np.sqrt((-np.sin(theta1_1) * nx + np.cos(theta1_1) * ny) ** 2 +
                            (-np.sin(theta1_1) * ox + np.cos(theta1_1) * oy) ** 2)
            theta5_2 = np.arctan2(S5_2, np.sin(theta1_1) * ax - np.cos(theta1_1) * ay)
            S5_3 = np.sqrt((-np.sin(theta1_2) * nx + np.cos(theta1_2) * ny) ** 2 +
                           (-np.sin(theta1_2) * ox + np.cos(theta1_2) * oy) ** 2)
            theta5_3 = np.arctan2(S5_3, np.sin(theta1_2) * ax - np.cos(theta1_2) * ay)
            S5_4 = -np.sqrt((-np.sin(theta1_2) * nx + np.cos(theta1_2) * ny) ** 2 +
                            (-np.sin(theta1_2) * ox + np.cos(theta1_2) * oy) ** 2)
            theta5_4 = np.arctan2(S5_4, np.sin(theta1_2) * ax - np.cos(theta1_2) * ay)
#当S5不等于0时可求出如下theta6,需要判断语句,并求解theta234的和， %求解theta2，
# %求解theta23的和,%求解theta4=theta234-theta23,%求解theta3=theta23-theta2,%将解分成8组解，以弧度为单位
            if abs(S5_1) > 1e-6:
                theta6_1 = np.arctan2((-np.sin(theta1_1) * ox + np.cos(theta1_1) * oy) / S5_1,
                                      (np.sin(theta1_1) * nx - np.cos(theta1_1) * ny) / S5_1)
                S234_1 = -az / S5_1
                C234_1 = -(np.cos(theta1_1) * ax + np.sin(theta1_1) * ay) / S5_1
                theta234_1 = np.arctan2(S234_1, C234_1)
                B1_1 = np.cos(theta1_1) * px + np.sin(theta1_1) * py - d5 * S234_1
                B2_1 = pz - d1 + d5 * C234_1
                A_1 = -2 * B2_1 * a2
                B_1 = 2 * B1_1 * a2
                C_1 = B1_1 ** 2 + B2_1 ** 2 + a2 ** 2 - a3 ** 2
                if A_1 ** 2 + B_1 ** 2 - C_1 ** 2 >= 0:
                    theta2_1 = np.arctan2(B_1, A_1) - np.arctan2(C_1, np.sqrt(A_1 ** 2 + B_1 ** 2 - C_1 ** 2))
                    theta2_2 = np.arctan2(B_1, A_1) - np.arctan2(C_1, -np.sqrt(A_1 ** 2 + B_1 ** 2 - C_1 ** 2))
                    theta23_1 = np.arctan2((B2_1 - a2 * np.sin(theta2_1)) / a3, (B1_1 - a2 * np.cos(theta2_1)) / a3)
                    theta23_2 = np.arctan2((B2_1 - a2 * np.sin(theta2_2)) / a3, (B1_1 - a2 * np.cos(theta2_2)) / a3)
                    theta4_1 = theta234_1 - theta23_1
                    theta4_2 = theta234_1 - theta23_2
                    theta3_1 = theta23_1 - theta2_1
                    theta3_2 = theta23_2 - theta2_2
                    itheta[k + 1, :] = [theta1_1, theta2_1, theta3_1, theta4_1, theta5_1, theta6_1]
                    itheta[k + 2, :] = [theta1_1, theta2_2, theta3_2, theta4_2, theta5_1, theta6_1]
                    k += 2

            if abs(S5_2) > 1e-6:
                theta6_2 = np.arctan2((-np.sin(theta1_1) * ox + np.cos(theta1_1) * oy) / S5_2,
                                      (np.sin(theta1_1) * nx - np.cos(theta1_1) * ny) / S5_2)
                S234_2 = -az / S5_2
                C234_2 = -(np.cos(theta1_1) * ax + np.sin(theta1_1) * ay) / S5_2
                theta234_2 = np.arctan2(S234_2, C234_2)
                B1_2 = np.cos(theta1_1) * px + np.sin(theta1_1) * py - d5 * S234_2
                B2_2 = pz - d1 + d5 * C234_2
                A_2 = -2 * B2_2 * a2
                B_2 = 2 * B1_2 * a2
                C_2 = B1_2 ** 2 + B2_2 ** 2 + a2 ** 2 - a3 ** 2
                if A_2 ** 2 + B_2 ** 2 - C_2 ** 2 >= 0:
                    theta2_3 = np.arctan2(B_2, A_2) - np.arctan2(C_2, np.sqrt(A_2 ** 2 + B_2 ** 2 - C_2 ** 2))
                    theta2_4 = np.arctan2(B_2, A_2) - np.arctan2(C_2, -np.sqrt(A_2 ** 2 + B_2 ** 2 - C_2 ** 2))
                    theta23_3 = np.arctan2((B2_2 - a2 * np.sin(theta2_3)) / a3, (B1_2 - a2 * np.cos(theta2_3)) / a3)
                    theta23_4 = np.arctan2((B2_2 - a2 * np.sin(theta2_4)) / a3, (B1_2 - a2 * np.cos(theta2_4)) / a3)
                    theta4_3 = theta234_2 - theta23_3
                    theta4_4 = theta234_2 - theta23_4
                    theta3_3 = theta23_3 - theta2_3
                    theta3_4 = theta23_4 - theta2_4
                    itheta[k + 1, :] = [theta1_1, theta2_3, theta3_3, theta4_3, theta5_2, theta6_2]
                    itheta[k + 2, :] = [theta1_1, theta2_4, theta3_4, theta4_4, theta5_2, theta6_2]
                    k += 2

            if abs(S5_3) > 1e-6:
                theta6_3 = np.arctan2((-np.sin(theta1_2) * ox + np.cos(theta1_2) * oy) / S5_3,
                                      (np.sin(theta1_2) * nx - np.cos(theta1_2) * ny) / S5_3)
                S234_3 = -az / S5_3
                C234_3 = -(np.cos(theta1_2) * ax + np.sin(theta1_2) * ay) / S5_3
                theta234_3 = np.arctan2(S234_3, C234_3)
                B1_3 = np.cos(theta1_2) * px + np.sin(theta1_2) * py - d5 * S234_3
                B2_3 = pz - d1 + d5 * C234_3
                A_3 = -2 * B2_3 * a2
                B_3 = 2 * B1_3 * a2
                C_3 = B1_3 ** 2 + B2_3 ** 2 + a2 ** 2 - a3 ** 2
                if A_3 ** 2 + B_3 ** 2 - C_3 ** 2 >= 0:
                    theta2_5 = np.arctan2(B_3, A_3) - np.arctan2(C_3, np.sqrt(A_3 ** 2 + B_3 ** 2 - C_3 ** 2))
                    theta2_6 = np.arctan2(B_3, A_3) - np.arctan2(C_3, -np.sqrt(A_3 ** 2 + B_3 ** 2 - C_3 ** 2))
                    theta23_5 = np.arctan2((B2_3 - a2 * np.sin(theta2_5)) / a3, (B1_3 - a2 * np.cos(theta2_5)) / a3)
                    theta23_6 = np.arctan2((B2_3 - a2 * np.sin(theta2_6)) / a3, (B1_3 - a2 * np.cos(theta2_6)) / a3)
                    theta4_5 = theta234_3 - theta23_5
                    theta4_6 = theta234_3 - theta23_6
                    theta3_5 = theta23_5 - theta2_5
                    theta3_6 = theta23_6 - theta2_6
                    itheta[k + 1, :] = [theta1_2, theta2_5, theta3_5, theta4_5, theta5_3, theta6_3]
                    itheta[k + 2, :] = [theta1_2, theta2_6, theta3_6, theta4_6, theta5_3, theta6_3]
                    k += 2

            if abs(S5_4) > 1e-6:
                theta6_4 = np.arctan2((-np.sin(theta1_2) * ox + np.cos(theta1_2) * oy) / S5_4,
                                      (np.sin(theta1_2) * nx - np.cos(theta1_2) * ny) / S5_4)
                S234_4 = -az / S5_4
                C234_4 = -(np.cos(theta1_2) * ax + np.sin(theta1_2) * ay) / S5_4
                theta234_4 = np.arctan2(S234_4, C234_4)
                B1_4 = np.cos(theta1_2) * px + np.sin(theta1_2) * py - d5 * S234_4
                B2_4 = pz - d1 + d5 * C234_4
                A_4 = -2 * B2_4 * a2
                B_4 = 2 * B1_4 * a2
                C_4 = B1_4 ** 2 + B2_4 ** 2 + a2 ** 2 - a3 ** 2
                if A_4 ** 2 + B_4 ** 2 - C_4 ** 2 >= 0:
                    theta2_7 = np.arctan2(B_4, A_4) - np.arctan2(C_4, np.sqrt(A_4 ** 2 + B_4 ** 2 - C_4 ** 2))
                    theta2_8 = np.arctan2(B_4, A_4) - np.arctan2(C_4, -np.sqrt(A_4 ** 2 + B_4 ** 2 - C_4 ** 2))
                    theta23_7 = np.arctan2((B2_4 - a2 * np.sin(theta2_7)) / a3, (B1_4 - a2 * np.cos(theta2_7)) / a3)
                    theta23_8 = np.arctan2((B2_4 - a2 * np.sin(theta2_8)) / a3, (B1_4 - a2 * np.cos(theta2_8)) / a3)
                    theta4_7 = theta234_4 - theta23_7
                    theta4_8 = theta234_4 - theta23_8
                    theta3_7 = theta23_7 - theta2_7
                    theta3_8 = theta23_8 - theta2_8
                    itheta[k + 1, :] = [theta1_2, theta2_7, theta3_7, theta4_7, theta5_4, theta6_4]
                    itheta[k + 2, :] = [theta1_2, theta2_8, theta3_8, theta4_8, theta5_4, theta6_4]
                    k += 2

            #将逆解变成角度单位
            if k > -1:
                itheta = itheta * 180 / np.pi
                for l in range(k):
                    for j in range(6):
                        if itheta[l, j] <= -180:
                            itheta[l, j] += 360
                        elif itheta[l, j] > 180:
                            itheta[l, j] -= 360
            else:
                error = 998  # 该位姿处于奇异位置有无穷解

        if error != 0:
            break

        # 解矩阵判断
        djie = []

        # 40 ~ -240
        if itheta[0, 5] > 40:
            itheta[0, 5] = -(360 - itheta[0, 5])

        if i == 0:
            Fjiao = np.append(Fjiao,np.array([itheta[0,:]]),axis=0)
            fjiaodu1 = -Fjiao[i,0]  # 底座 - 轴6
            fjiaodu2 = Fjiao[i,1] + 90  # 轴5 - 可能存在问题
            fjiaodu3 = -Fjiao[i,2] - 90  # 轴4 - -可能存在问题
            fjiaodu4 = +Fjiao[i,3]  # 轴3
            fjiaodu5 = -Fjiao[i,4] + 90  # 轴2
            fjiaodu6 = -Fjiao[i,5]  # 执行器末端 - 轴1
        else:
            a, b = itheta.shape
            for m in range(a):
                chazhi = abs(itheta[m, 0] - Fjiao[i - 1, 0]) + abs(itheta[m, 1] - Fjiao[i - 1, 1]) + abs(
                    itheta[m, 2] - Fjiao[i - 1, 2]) + abs(itheta[m, 3] - Fjiao[i - 1, 3]) + abs(
                    itheta[m, 4] - Fjiao[i - 1, 4]) + abs(itheta[m, 5] - Fjiao[i - 1, 5])
                djie.append(chazhi)
            djie = np.array(djie)
            minx = np.argmin(djie)
            # Fjiao = Fjiao.tolist()
            # Fjiao = np.row_stack((Fjiao,
            #                       [itheta[minx, 0], itheta[minx, 1], itheta[minx, 2], itheta[minx, 3], itheta[minx, 4],
            #                        itheta[minx, 5]]))
            Fjiao = np.append(Fjiao,
                                  np.array([itheta[minx,:]]),axis=0)
            fjiaodu1 = -Fjiao[i, 0] # 底座 - 轴6
            fjiaodu2 = Fjiao[i, 1] + 90 # 轴5 - 可能存在问题
            fjiaodu3 = -Fjiao[i, 2] - 90 # 轴4 - -可能存在问题
            fjiaodu4 = +Fjiao[i, 3] #轴3
            fjiaodu5 = -Fjiao[i, 4] + 90 # 轴2
            fjiaodu6 = -Fjiao[i, 5] # 执行器末端 - 轴1
        angle = np.append(angle, (np.array([[fjiaodu1, fjiaodu2, fjiaodu3, fjiaodu4, fjiaodu5, fjiaodu6]])),axis=0)

    # angles = angle * np.pi / 180  #全部完成后，转化为弧度制
    angles = Fjiao * np.pi / 180  # Fjiao没有经过加减90°转化，用于某些场合，全部完成后，转化为弧度制
    # angles=Fjiao
    return angles

"""
    目标物体检测
    使用的为canny边缘检测
"""
def Detect_shapes(image):
    # 获取图像尺寸
    height, width, _ = image.shape

    # 将图像转换为灰度图
    gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)

    # 图像增强：直方图均衡化
    enhanced_image = cv2.equalizeHist(gray)
    # 去除非边缘噪声：使用形态学操作
    kernel = np.ones((3, 3), np.uint8)
    morph_image = cv2.morphologyEx(enhanced_image, cv2.MORPH_OPEN, kernel)
    # 边缘增强：使用Laplacian算子
    laplacian = cv2.Laplacian(morph_image, cv2.CV_64F)
    # edges = cv2.Canny(np.uint8(laplacian), 60, 200, apertureSize=5, L2gradient=True)

    # 使用Canny边缘检测
    edges = cv2.Canny(gray, 60, 200, apertureSize=3, L2gradient=True)

    # 检测轮廓
    contours, _= cv2.findContours(edges, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)

    # 过滤小边缘
    min_contour_length = 60  # 设置最小边缘长度阈值
    filtered_contours = [cnt for cnt in contours if cv2.arcLength(cnt, closed=True) > min_contour_length]

    # 新建一个列表，用于存储有效的轮廓
    valid_contours = []

    # 建一个变量，存储第一个要操作的大洞
    max_contour = []
    max_contour_a = 0
    max_contour_b = 0
    # 循环检测到的轮廓
    for contour in filtered_contours:
        # 拟合椭圆
        if len(contour) >= 5:
            ellipse = cv2.fitEllipse(contour)
            (x, y), (a, b), angle = ellipse
            # 计算椭圆的四个顶点
            box = cv2.boxPoints(ellipse)
            # 排除过小的椭圆
            if ellipse[1][0] > 60 and ellipse[1][1] > 60:
                # 检查椭圆的四个顶点是否都在图像内
                inside = True
                for point in box:
                    if point[0] < 0 or point[0] >= width or point[1] < 0 or point[1] >= height:
                        inside = False
                        break
                if inside:
                    valid_contours.append(contour)
                    if a * b > max_contour_a * max_contour_b:  # 找出面积最大的椭圆
                        max_contour_a = a
                        max_contour_b = b
                        max_contour = contour

    # 绘制轮廓
    cv2.drawContours(image, valid_contours, -1, (100, 50, 0), 3)
    # 重新拟合剩余的轮廓
    i = 0
    for contour in valid_contours:
        ellipse = cv2.fitEllipse(contour)
        # 绘制椭圆
        cv2.ellipse(image, ellipse, (0, 255, 0), 2)
        # 绘制中心点
        center = (int(ellipse[0][0]), int(ellipse[0][1]))
        cv2.circle(image, center, 5, (0, 0, 255), -1)
        cv2.putText(image, str(i), center, cv2.FONT_HERSHEY_PLAIN, 1, (100, 0, 255), 1)
        i += 1
    # 画出最大图形
    if len(max_contour) > 0:
        # image1=image
        ell = cv2.fitEllipse(max_contour)
        cv2.ellipse(image, ell, (0, 0, 255), 5)
    Allimg = np.hstack((enhanced_image, morph_image, laplacian, edges))
    cv2.imshow('', Allimg)
    # 显示带有标识的图像
    cv2.imshow("Detected Ellipses", image)
    cv2.waitKey(0)
    cv2.destroyAllWindows()
